#pragma once
#ifndef SAMPLER_H
#define SAMPLER_H

#include "basic_typedef.h"
#include "CollisionDetector.h"

class Sampler
{
public:
    
    ////////////////////////
    // CTORS
    ////////////////////////
    /*	Parameters:
            robot	- the robot
            Room	- describes the boundary of the scenario
            CollisionDetector	- Collision detection black box
    */

    Sampler(Polygon_2 robot, Room room, CollisionDetector *col, double seed = -1) : 
      m_robot(robot), m_col(col), m_room(room)
    {	
        tl = m_room.first;
        br = m_room.second;
        xmin = tl.x().exact().to_double();
        xmax = br.x().exact().to_double();
        ymax = br.y().exact().to_double();
        ymin = tl.y().exact().to_double();
        lenx = xmax - xmin;
        leny = ymax - ymin;

        if (seed == -1)
            seed = time(NULL);
        srand ( seed );
        
        //no needed to add the room - it's already an obstacle
    }

    ~Sampler(void)
    {

    }

    ////////////////////////
    // Queries
    ////////////////////////
    
    /*	Returns a point that represents a valid (free) 
        configuration of the robot. */
    Point_2 generate_sample()
    {
        {
            while(true)
            {
                // sample a point in the configuration space
                Point_2 p = generate_sample_no_obstacles();

                // validate sample using a collision detector
                if (m_col->valid_conf(p))
                    return p;
            }
        }
    }

private:

    /*	Samples a point in the configuration space of the robot
        defined by the bounding box of the scenario.
        This point can be valid or invalid.	*/
    Point_2 generate_sample_no_obstacles()
    {
        double randx = xmin + lenx * ((double)rand())/((double)RAND_MAX);
        double randy = ymin + leny * ((double)rand())/((double)RAND_MAX);

        Point_2 p(randx,randy);

        return p;
    }

    ////////////////////////
    // Data Members
    ////////////////////////
    Polygon_2	m_robot;	// The robot
    Polygon_2	m_free;	    // The free space (dictated by the room)
        
    Room		m_room;		// Represents the boundaries of the scenario
    CollisionDetector	*m_col;	// Collision detector

    // room variables
    Point_2 tl;
    Point_2 br;
    double xmin;
    double xmax;
    double ymin;
    double ymax;
    double lenx;
    double leny;
};
#endif